/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.ShooterStopMotors;

/**
 *
 * @author Eddie
 */
public class BallShooter extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    Jaguar leftShooterMotor;
    Jaguar rightShooterMotor;

    public boolean shooterState; 
    
    public BallShooter() {
        super("BallShooter");
        
        leftShooterMotor = new Jaguar(RobotMap.SHOOTER_LEFT);
        rightShooterMotor = new Jaguar(RobotMap.SHOOTER_RIGHT);

        shooterState = false;
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new ShooterStopMotors());
    }
    
     public void spinWheels(boolean forward) {
        int leftMultiplier = 1;
        int rightMultiplier = 1;

        if(RobotMap.SHOOTER_LEFT_REVERSED) {
            leftMultiplier = -1;
        }

        if(RobotMap.SHOOTER_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }

        if(forward) {
            leftShooterMotor.set(RobotMap.SHOOTER_MAX_SPEED * leftMultiplier);
            rightShooterMotor.set(RobotMap.SHOOTER_MAX_SPEED * rightMultiplier);
        } else {
            leftShooterMotor.set(-RobotMap.SHOOTER_MAX_SPEED * leftMultiplier);
            rightShooterMotor.set(-RobotMap.SHOOTER_MAX_SPEED * rightMultiplier);
        }
    }
    
    public void spinWheels(double speed) {
        int leftMultiplier = 1;
        int rightMultiplier = 1;
        
        if(RobotMap.SHOOTER_LEFT_REVERSED) {
            leftMultiplier = -1;
        }
        
        if(RobotMap.SHOOTER_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }
                  
        leftShooterMotor.set(speed * leftMultiplier);
        rightShooterMotor.set(speed * rightMultiplier);   
    }
    
    public void stopWheels() {
        if(!shooterState) {
            leftShooterMotor.set(0.0);
            rightShooterMotor.set(0.0);
        } else {
            spinWheels(RobotMap.SHOOTER_MAX_SPEED);
        }
    }
    
    public void changeSpeed(double change) {
        if(RobotMap.SHOOTER_MAX_SPEED + change <= 1 && RobotMap.SHOOTER_MAX_SPEED >=0) {
            RobotMap.SHOOTER_MAX_SPEED += change;
        } else {
           System.out.println("BallShooter - Value greater than 1 or less than 0");
        }
    }

    public void setSpeed(double speed) {
        if(speed > 0 && speed <= 1) {
            RobotMap.SHOOTER_MAX_SPEED = speed;
        }
    }

    public void updateStatus() {
        SmartDashboard.putDouble("Shooter Left Motor", leftShooterMotor.getSpeed());
        SmartDashboard.putDouble("Shooter Right Motor", rightShooterMotor.getSpeed());
    }
}
